/**********************************************************************
*  Copyright (c) 2007 - Victor Jacobs - victor.jacobs@gmail.com
*
*  Permission is hereby granted, free of charge, to any person
*  obtaining a copy of this software and associated documentation
*  files (the "Software"), to deal in the Software without
*  restriction, including without limitation the rights to use,
*  copy, modify, merge, publish, distribute, sublicense, and/or sell
*  copies of the Software, and to permit persons to whom the
*  Software is furnished to do so, subject to the following
*  conditions:
*
*  The above copyright notice and this permission notice shall be
*  included in all copies or substantial portions of the Software.
*
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
*  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
*  OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
*  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
*  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
*  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
*  OTHER DEALINGS IN THE SOFTWARE.
**********************************************************************/

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using Tao.OpenGl;
using Tao.Ode;
using Vj.MyMath;
using Vj.ShapeLibrary;
using Vj.OdeObjectLibrary;
using WalkingApp;

namespace Vj.WalkingApp
{
    public partial class MainForm : Form
    {
        #region Data Members
        
        private static IntPtr world;
        private IntPtr space;

        private Grid floor;
        private Hexapod hexapod;

        private static IntPtr contactgroup;
        private Ode.dContactGeom[] contacts;

        #region Support Object Properties
        private Ode.dVector3 supportDimension;
        private Ode.dVector3 supportPosition;
        private IntPtr supportBody;
        private IntPtr supportGeom;
        #endregion

        #endregion

        public MainForm()
        {
            InitializeComponent();
        }

        #region On Load

        private void simpleOpenGlControl1_Load(object sender, EventArgs e)
        {
            simpleOpenGlControl1.InitializeContexts();
            simpleOpenGlControl1.AutoSwapBuffers = true;

            InitGL();
            SetLights();

            InitializeODEWorld();

            simpleOpenGlControl1_Resize(null, null);
        }

        #endregion

        #region Initialization

        private void simpleOpenGlControl1_Resize(object sender, EventArgs e)
        {
            int w = simpleOpenGlControl1.Width;
            int h = simpleOpenGlControl1.Height;

            float ratio = ((float)w / (float)h);

            // set viewport to current window size
            Gl.glViewport(0, 0, w, h);

            // set up prespective projection
            Gl.glMatrixMode(Gl.GL_PROJECTION);
            Gl.glLoadIdentity();
            Glu.gluPerspective(90, ratio, 1, 500);
            //Glu.gluLookAt(0.0, 0.0, 30.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

            // set up model view matrix
            Gl.glMatrixMode(Gl.GL_MODELVIEW);
            Gl.glLoadIdentity();
        }

        private static bool InitGL()
        {
            //Gl.glShadeModel(Gl.GL_SMOOTH);
            Gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
            Gl.glClearDepth(1);
            Gl.glEnable(Gl.GL_DEPTH_TEST);
            Gl.glDepthFunc(Gl.GL_LEQUAL);
            Gl.glHint(Gl.GL_PERSPECTIVE_CORRECTION_HINT, Gl.GL_NICEST);
            Gl.glEnable(Gl.GL_LIGHTING);
            Gl.glEnable(Gl.GL_LIGHT0);
            Gl.glEnable(Gl.GL_LIGHT1);
            Gl.glEnable(Gl.GL_LIGHT2);
            Gl.glEnable(Gl.GL_NORMALIZE);

            return true;
        }

        float[] lightPosition0;
        float[] lightPosition1;
        float[] lightPosition2;

        private void SetLights()
        {
            // global light
            float[] lmodel_ambient = { 0.6f, 0.6f, 0.6f, 1.0f };
            Gl.glLightModelfv(Gl.GL_LIGHT_MODEL_AMBIENT, lmodel_ambient);

            lightPosition0 = new float[]{ 0.0f, 10.0f, 0.0f, 1.0f };
            float[] lightAmbient0 = { 0.75f, 0.75f, 0.75f, 1.0f };
            float[] lightDiffuse0 = { 1.0f, 1.0f, 1.0f, 1.0f };
            float[] lightSpecular0 = { 0.75f, 0.75f, 0.75f, 1.0f };

            lightPosition1 = new float[]{ 10.0f, 10.0f, 0.0f, 1.0f };
            float[] lightAmbient1 = { 0.4f, 0.4f, 0.4f, 1.0f };
            float[] lightDiffuse1 = { 0.6f, 0.6f, 0.6f, 1.0f };
            float[] lightSpecular1 = { 0.6f, 0.6f, 0.6f, 1.0f };

            lightPosition2 = new float[] { -22.0f, 22.0f, 0.0f, 1.0f };
            float[] lightAmbient2 = { 0.4f, 0.4f, 0.4f, 1.0f };
            float[] lightDiffuse2 = { 0.6f, 0.6f, 0.6f, 1.0f };
            float[] lightSpecular2 = { 0.6f, 0.6f, 0.6f, 1.0f };

            Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_POSITION, lightPosition0);
            Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_SPECULAR, lightSpecular0);
            Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_AMBIENT, lightAmbient0);
            Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_DIFFUSE, lightDiffuse0);

            Gl.glLightfv(Gl.GL_LIGHT1, Gl.GL_POSITION, lightPosition1);
            Gl.glLightfv(Gl.GL_LIGHT1, Gl.GL_SPECULAR, lightSpecular1);
            Gl.glLightfv(Gl.GL_LIGHT1, Gl.GL_AMBIENT, lightAmbient1);
            Gl.glLightfv(Gl.GL_LIGHT1, Gl.GL_DIFFUSE, lightDiffuse1);

            Gl.glLightfv(Gl.GL_LIGHT2, Gl.GL_POSITION, lightPosition2);
            Gl.glLightfv(Gl.GL_LIGHT2, Gl.GL_SPECULAR, lightSpecular2);
            Gl.glLightfv(Gl.GL_LIGHT2, Gl.GL_AMBIENT, lightAmbient2);
            Gl.glLightfv(Gl.GL_LIGHT2, Gl.GL_DIFFUSE, lightDiffuse2);

        }

        #endregion

        #region ODE methods

        private FourierWalk fourierWalker;

        private void InitializeODEWorld()
        {
            world = Ode.dWorldCreate();
            space = Ode.dHashSpaceCreate(IntPtr.Zero);

            contactgroup = Ode.dJointGroupCreate(0);
            contacts = new Ode.dContactGeom[8];

            floor = new Grid(new Vector(0.0, 0.0, 0.1), 40, 20, false);
            Ode.dCreatePlane(space, 0.0f, 1.0f, 0.0f, 0.0f);

            Ode.dWorldSetGravity(world, 0.0f, -9.81f, 0.0f);

            Ode.dWorldSetERP(world, 0.2f);
            Ode.dWorldSetCFM(world, 1e-5f);
            //Ode.dWorldSetAutoDisableFlag(world, 1);
            Ode.dWorldSetContactMaxCorrectingVel(world, 0.1f);
            Ode.dWorldSetContactSurfaceLayer(world, 0.00001f);

            //hexapod = new Hexapod(world, space, 1.0f, 10.0f,
            //    new Ode.dVector3(0.0f, 0.0f, 0.0f),
            //    new Ode.dVector3(0.0f, 0.0f, 0.0f),
            //    new Ode.dVector3(153.0f/255.0f, 51.0f/255.0f, 0.0f));

            fourierWalker = new FourierWalk(world, space);

            //InitializeSupportObject();
        }

        private void InitializeSupportObject()
        {
            //if (supportBody != new IntPtr(0))
            //{
            //    Ode.dGeomDestroy(supportGeom);
            //    Ode.dBodyDestroy(supportBody);
            //}

            supportPosition =
                new Ode.dVector3((float)supportXLocUpDown.Value,
                                 (float)supportYLocUpDown.Value,
                                 (float)supportZLocUpDown.Value);

            supportDimension =
                new Ode.dVector3((float)supportXSizeUpDown.Value,
                                 (float)supportYSizeUpDown.Value,
                                 (float)supportZSizeUpDown.Value);

            supportBody = Ode.dBodyCreate(world);
            Ode.dBodySetPosition(supportBody, supportPosition.X, supportPosition.Y, supportPosition.Z);
            supportGeom = Ode.dCreateBox(space, supportDimension.X, supportDimension.Y, supportDimension.Z);
            Ode.dGeomSetBody(supportGeom, supportBody);

            Ode.dMass objMass2 = new Ode.dMass();
            Ode.dMassSetBox(ref objMass2, 10, supportDimension.X, supportDimension.Y, supportDimension.Z);
            objMass2.mass = 1;
            Ode.dBodySetMass(supportBody, ref objMass2);

        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            if (currentTime < (int)simTimeUpDown.Value)
            {
                currTimeTextBox.Text = currentTime.ToString();

                currTimeTextBox.Refresh();

                simpleOpenGlControl1.Refresh();

                //if (checkBox1.Checked)
                //    ApplyTotalTorque();

                //Ode.dBodyAddForce(hexapod.BodyArray[0],
                //    (float)xForceUpDown.Value,
                //    (float)yForceUpDown.Value,
                //    (float)zForceUpDown.Value);

                fourierWalker.IncrementAgents();

                Ode.dSpaceCollide(space, new IntPtr(), new Ode.dNearCallback(near));
                Ode.dWorldStep(world, deltaTime);
                Ode.dJointGroupEmpty(contactgroup);

                currentTime += deltaTime;
            }
            else
            {
                currentTime = 0.0f;
                timer1.Enabled = false;
            }
        }

        static void near(IntPtr data, IntPtr o1, IntPtr o2)
        {
            IntPtr b1 = Ode.dGeomGetBody(o1);
            IntPtr b2 = Ode.dGeomGetBody(o2);

            int MAX_COLLISIONS = 10;

            Ode.dContactGeom[] contactGeoms = new Ode.dContactGeom[MAX_COLLISIONS];

            int numc = Ode.dCollide(o1, o2, MAX_COLLISIONS, contactGeoms,
                System.Runtime.InteropServices.Marshal.SizeOf(contactGeoms[0]));

            Ode.dContact[] contact = new Ode.dContact[numc];

            for (int i = 0; i < numc; i++)
            {
                contact[i].surface.mode =
                    (int)Ode.dContactFlags.dContactBounce |
                    (int)Ode.dContactFlags.dContactSoftCFM;

                //contact[i].surface.mu = 2.1f;
                //contact[i].surface.mu2 = 2.1f;
                //contact[i].surface.bounce = 0.1f;
                //contact[i].surface.bounce_vel = 0.1f;
                //contact[i].surface.soft_cfm = 0;

                contact[i].surface.mu = 20.1f;
                contact[i].surface.mu2 = 2.1f;
                contact[i].surface.bounce = 0.1f;
                contact[i].surface.bounce_vel = 0.1f;
                contact[i].surface.soft_cfm = 0;

                contact[i].geom = contactGeoms[i];

                IntPtr joint =
                    Ode.dJointCreateContact(world, contactgroup, ref contact[i]);

                Ode.dJointAttach(joint, b1, b2);
            }
        }

        private void ApplyTotalTorque()
        {
            // ---  Left Upper Joints  ---

            hexapod.SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Upper,
                new float[] { (float)L1Upper1UpDown.Value, 0.0f });
            hexapod.SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Upper,
                new float[] { 0.0f, (float)L1Upper2UpDown.Value });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Upper,
                new float[] { (float)L2Upper1UpDown.Value, 0.0f });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Upper,
                new float[] { 0.0f, (float)L2Upper2UpDown.Value });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Upper,
                new float[] { (float)L3Upper1UpDown.Value, 0.0f });
            hexapod.SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Upper,
                new float[] { 0.0f, (float)L3Upper2UpDown.Value });

            // ---  Left Lower Joints  ---
            hexapod.SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Lower,
                new float[] { (float)L1LowerUpDown.Value });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Lower,
                new float[] { (float)L2LowerUpDown.Value });
            hexapod.SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Lower,
                new float[] { (float)L3LowerUpDown.Value });

            // ---  Right Upper Joints ---

            hexapod.SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Upper,
                new float[] { (float)R1Upper1UpDown.Value, 0.0f });
            hexapod.SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Upper,
                new float[] { 0.0f, (float)R1Upper2UpDown.Value });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Upper,
                new float[] { (float)R2Upper1UpDown.Value, 0.0f });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Upper,
                new float[] { 0.0f, (float)R2Upper2UpDown.Value });
            hexapod.SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Upper,
                new float[] { (float)R3Upper1UpDown.Value, 0.0f });
            hexapod.SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Upper,
                new float[] { 0.0f, (float)R2Upper2UpDown.Value });

            // ---  Right Lower Joints ---

            hexapod.SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Lower,
                new float[] { (float)R1LowerUpDown.Value });
            hexapod.SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Lower,
                new float[] { (float)R2LowerUpDown.Value });
            hexapod.SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Lower,
                new float[] { (float)R3LowerUpDown.Value });

        }

        #endregion

        #region Paint

        private void simpleOpenGlControl1_Paint(object sender, PaintEventArgs e)
        {
            Gl.glClear(Gl.GL_COLOR_BUFFER_BIT | Gl.GL_DEPTH_BUFFER_BIT);
            Gl.glLoadIdentity();

            Gl.glPushMatrix();

            Gl.glTranslated(xTransScrollBar.Value,
                            yTransScrollBar.Value,
                            zTransScrollBar.Value - 15.0f);

            Gl.glRotatef(xRotScrollBar.Value, 1.0f, 0.0f, 0.0f);
            Gl.glRotatef(yRotScrollBar.Value, 0.0f, 1.0f, 0.0f);
            Gl.glRotatef(zRotScrollBar.Value, 0.0f, 0.0f, 1.0f);

            Gl.glLightfv(Gl.GL_LIGHT0, Gl.GL_POSITION, lightPosition0);

            SetLights();

            #region Draw Lights
            if (drawLightsCheckBox.Checked)
            {
                float[] mat_emission = { 0.5f, 0.5f, 0.5f, 1.0f };
                Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_EMISSION, mat_emission);

                Sphere light0 = new Sphere();
                light0.Location = new Vector(lightPosition0);
                light0.Color = new Vector(1.0, 1.0, 1.0);
                light0.Radius = 0.5;
                light0.DefineShape(1, 1);
                light0.Draw();

                Sphere light1 = new Sphere();
                light1.Location = new Vector(lightPosition1);
                light1.Color = new Vector(1.0, 1.0, 1.0);
                light1.Radius = 0.5;
                light1.DefineShape(1, 1);
                light1.Draw();

                Sphere light2 = new Sphere();
                light2.Location = new Vector(lightPosition2);
                light2.Color = new Vector(1.0, 1.0, 1.0);
                light2.Radius = 0.5;
                light2.DefineShape(1, 1);
                light2.Draw();

                float[] mat_emission2 = { 0.0f, 0.0f, 0.0f, 1.0f };
                Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_EMISSION, mat_emission2);

            }
            #endregion
            
            floor.Draw();

            //hexapod.Render();

            fourierWalker.Draw();

            if(drawSupportObjCheckBox.Checked)
                DrawSupportBox();

            Gl.glPopMatrix();
        }

        private void DrawSupportBox()
        {
            Gl.glPushMatrix();

            supportPosition = Ode.dBodyGetPosition(supportBody);

            Box.Draw(
                new Vector(supportPosition.X, supportPosition.Y, supportPosition.Z),
                new Vector(supportDimension.X, supportDimension.Y, supportDimension.Z),
                new Vector(1.0, 1.0, 1.0));

            Gl.glPopMatrix();
        }

        #endregion

        #region Button Handlers

        private float deltaTime = 0.04f;
        private float currentTime = 0.0f;

        private void startButton_Click(object sender, EventArgs e)
        {
            //currentTime = 0.0f;
            timer1.Enabled = true;
        }

        private void resetButton_Click(object sender, EventArgs e)
        {
            currentTime = 0.0f;
            InitializeODEWorld();
            if(drawSupportObjCheckBox.Checked)
                InitializeSupportObject();
            simpleOpenGlControl1.Refresh();

            this.Refresh();
        }

        private void closeButton_Click(object sender, EventArgs e)
        {
            this.Close();
        }

        private void stopButton_Click(object sender, EventArgs e)
        {
            timer1.Enabled = false;
        }

        #endregion

        #region View Controls

        private void xRotScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            xRotTextBox.Text = xRotScrollBar.Value.ToString();
            simpleOpenGlControl1.Refresh();
        }

        private void yRotScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            yRotTextBox.Text = yRotScrollBar.Value.ToString();
            simpleOpenGlControl1.Refresh();
        }

        private void zRotScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            zRotTextBox.Text = zRotScrollBar.Value.ToString();
            simpleOpenGlControl1.Refresh();
        }

        private void xTransScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            xTransTextBox.Text = xTransScrollBar.Value.ToString();
            simpleOpenGlControl1.Refresh();
        }

        private void yTransScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            yTransTextBox.Text = yTransScrollBar.Value.ToString();
            simpleOpenGlControl1.Refresh();
        }

        private void zTransScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            zTransTextBox.Text = zTransScrollBar.Value.ToString();
            simpleOpenGlControl1.Refresh();
        }

        private void viewResetButton_Click(object sender, EventArgs e)
        {
            xRotScrollBar.Value = 0;
            yRotScrollBar.Value = 0;
            zRotScrollBar.Value = 0;

            xRotTextBox.Text = "0";
            yRotTextBox.Text = "0";
            zRotTextBox.Text = "0";

            xTransScrollBar.Value = 0;
            yTransScrollBar.Value = 0;
            zTransScrollBar.Value = 0;

            xTransTextBox.Text = "0";
            yTransTextBox.Text = "0";
            zTransTextBox.Text = "0";

            this.Refresh();
        }

        #endregion

        #region Joint Force Handlers

        // ---  Left Upper Joints  ---
        private void L1Upper1UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Upper,
                new float[] { (float)L1Upper1UpDown.Value, 0.0f });
        }

        private void L1Upper2UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Upper,
                new float[] { 0.0f, (float)L1Upper2UpDown.Value });
        }

        private void L2Upper1UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Upper,
                new float[] { (float)L2Upper1UpDown.Value, 0.0f });
        }

        private void L2Upper2UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Upper,
                new float[] { 0.0f, (float)L2Upper2UpDown.Value });
        }

        private void L3Upper1UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Upper,
                new float[] { (float)L3Upper1UpDown.Value, 0.0f });
        }

        private void L3Upper2UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Upper,
                new float[] { 0.0f, (float)L3Upper2UpDown.Value });
        }

        // ---  Left Lower Joints  ---
        private void L1LowerUpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Lower,
                new float[] { (float)L1LowerUpDown.Value });
        }

        private void L2LowerUpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Lower,
                new float[] { (float)L2LowerUpDown.Value });
        }

        private void L3LowerUpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Lower,
                new float[] { (float)L3LowerUpDown.Value });
        }

        // ---  Right Upper Joints ---

        private void R1Upper1UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Upper,
                new float[] { (float)R1Upper1UpDown.Value, 0.0f });
        }

        private void R1Upper2UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Upper,
                new float[] { 0.0f, (float)R1Upper2UpDown.Value });
        }

        private void R2Upper1UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Upper,
                new float[] { (float)R2Upper1UpDown.Value, 0.0f });
        }

        private void R2Upper2UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Upper,
                new float[] { 0.0f, (float)R2Upper2UpDown.Value });
        }

        private void R3Upper1UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Upper,
                new float[] { (float)R3Upper1UpDown.Value, 0.0f });
        }

        private void R3Upper2UpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Upper,
                new float[] { 0.0f, (float)R2Upper2UpDown.Value });
        }

        // ---  Right Lower Joints ---

        private void R1LowerUpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Lower,
                new float[] { (float)R1LowerUpDown.Value });
        }

        private void R2LowerUpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Lower,
                new float[] { (float)R2LowerUpDown.Value });
        }

        private void R3LowerUpDown_ValueChanged(object sender, EventArgs e)
        {
            hexapod.SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Lower,
                new float[] { (float)R3LowerUpDown.Value });
        }

        #endregion

        #region Support Object Handlers
        private void supportXLocUpDown_ValueChanged(object sender, EventArgs e)
        {
            InitializeSupportObject();
            this.Refresh();
        }

        private void supportYLocUpDown_ValueChanged(object sender, EventArgs e)
        {
            InitializeSupportObject();
            this.Refresh();
        }

        private void supportZLocUpDown_ValueChanged(object sender, EventArgs e)
        {
            InitializeSupportObject();
            this.Refresh();
        }

        private void supportXSizeUpDown_ValueChanged(object sender, EventArgs e)
        {
            InitializeSupportObject();
            this.Refresh();
        }

        private void supportYSizeUpDown_ValueChanged(object sender, EventArgs e)
        {
            InitializeSupportObject();
            this.Refresh();
        }

        private void supportZSizeUpDown_ValueChanged(object sender, EventArgs e)
        {
            InitializeSupportObject();
            this.Refresh();
        }

        private void drawSupportObjCheckBox_CheckedChanged(object sender, EventArgs e)
        {
            float[] mat_ambient = { 0.0f, 0.0f, 0.0f, 1.0f };
            float[] mat_diffuse = { 0.5f, 0.0f, 0.0f, 1.0f };
            float[] mat_specular = { 0.7f, 0.6f, 0.6f, 1.0f };
            float[] mat_shininess = { 0.25f };

            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SPECULAR, mat_specular);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_DIFFUSE, mat_diffuse);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_AMBIENT, mat_ambient);
            Gl.glMaterialfv(Gl.GL_FRONT, Gl.GL_SHININESS, mat_shininess);

            if (drawSupportObjCheckBox.Checked)
            {
                InitializeSupportObject();
            }
            else
            {
                Ode.dBodyDestroy(supportBody);
                Ode.dGeomDestroy(supportGeom);
            }
            this.Refresh();
        }
        #endregion

        private void drawLightsCheckBox_CheckedChanged(object sender, EventArgs e)
        {
            simpleOpenGlControl1.Refresh();
        }

 




    }
}